#ifndef POINTCLOUD_PLANNER_HPP
#define POINTCLOUD_PLANNER_HPP

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree_search.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include <queue>
#include <unordered_map>
#include <functional>
#include "auto_navigation/planning/planner_interface.hpp"

namespace auto_navigation {
namespace planning {

/**
 * @brief 基于点云的A*规划器类
 */
class PointCloudPlanner : public PlannerInterface {
public:
    /**
     * @brief 构造函数
     */
    PointCloudPlanner();
    
    /**
     * @brief 析构函数
     */
    virtual ~PointCloudPlanner();
    
    /**
     * @brief 初始化规划器
     * @param nh ROS节点句柄
     * @return 是否成功初始化
     */
    virtual bool initialize(ros::NodeHandle& nh) override;
    
    /**
     * @brief 设置地图
     * @param map 占用栅格地图
     * @return 是否成功设置
     */
    virtual bool setMap(const nav_msgs::OccupancyGrid& map) override;
    
    /**
     * @brief 设置点云地图
     * @param cloud 点云地图
     * @return 是否成功设置
     */
    bool setPointCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud);
    
    /**
     * @brief 规划路径
     * @param start 起始位姿
     * @param goal 目标位姿
     * @param path 输出的路径
     * @return 是否成功规划
     */
    virtual bool plan(const geometry_msgs::PoseStamped& start,
                     const geometry_msgs::PoseStamped& goal,
                     nav_msgs::Path& path) override;
    
    /**
     * @brief 将世界坐标转换为网格坐标
     * @param wx 世界坐标x
     * @param wy 世界坐标y
     * @param gx 输出的网格坐标x
     * @param gy 输出的网格坐标y
     * @return 是否成功转换
     */
    bool worldToGrid(double wx, double wy, int& gx, int& gy) const;
    
    /**
     * @brief 将网格坐标转换为世界坐标
     * @param gx 网格坐标x
     * @param gy 网格坐标y
     * @param wx 输出的世界坐标x
     * @param wy 输出的世界坐标y
     */
    void gridToWorld(int gx, int gy, double& wx, double& wy) const;
    
    /**
     * @brief 检查网格单元是否有效
     * @param x 网格坐标x
     * @param y 网格坐标y
     * @return 是否有效
     */
    bool isValidCell(int x, int y) const;
    
    /**
     * @brief 检查网格单元是否被占用
     * @param x 网格坐标x
     * @param y 网格坐标y
     * @return 是否被占用
     */
    bool isOccupied(int x, int y) const;
    
private:
    /**
     * @brief 网格单元结构体
     */
    struct GridCell {
        int x;
        int y;
        
        bool operator==(const GridCell& other) const {
            return x == other.x && y == other.y;
        }
        
        bool operator<(const GridCell& other) const {
            return x < other.x || (x == other.x && y < other.y);
        }
    };
    
    /**
     * @brief 网格单元哈希函数
     */
    struct GridCellHash {
        std::size_t operator()(const GridCell& cell) const {
            return std::hash<int>()(cell.x) ^ (std::hash<int>()(cell.y) << 1);
        }
    };
    
    /**
     * @brief 计算两个网格单元之间的启发式距离
     * @param x1 起点网格坐标x
     * @param y1 起点网格坐标y
     * @param x2 终点网格坐标x
     * @param y2 终点网格坐标y
     * @return 启发式距离
     */
    double heuristic(int x1, int y1, int x2, int y2) const;
    
    /**
     * @brief 重建路径
     * @param came_from 路径来源映射表
     * @param current 当前网格单元
     * @param path 输出的路径
     */
    void reconstructPath(const std::unordered_map<GridCell, GridCell, GridCellHash>& came_from,
                         const GridCell& current,
                         nav_msgs::Path& path) const;
    
    /**
     * @brief 创建2D栅格地图
     */
    void createGridMap();
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;  ///< 点云地图
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr octree_;  ///< 八叉树搜索结构
    bool cloud_received_;            ///< 是否接收到点云
    double resolution_;              ///< 栅格分辨率
    double min_height_;              ///< 最小高度
    double max_height_;              ///< 最大高度
    double obstacle_radius_;         ///< 障碍物半径
    int width_;                      ///< 栅格地图宽度
    int height_;                     ///< 栅格地图高度
    double origin_x_;                ///< 栅格地图原点x坐标
    double origin_y_;                ///< 栅格地图原点y坐标
    std::vector<std::vector<bool>> grid_map_;  ///< 2D栅格地图
};

} // namespace planning
} // namespace auto_navigation

#endif // POINTCLOUD_PLANNER_HPP 